// -*- Mode: C++ -*-
// Ditching
Command SetPos(Real position[3]);
Command SetMode(String mode);
Command pprint(...);
Real[3]    Command GetMissionWP(Integer index);

Boolean Lookup requireDitchGuidance;
Boolean Lookup ditchingComplete;
Boolean Lookup resetDitching;
Real    Lookup resolutionSpeed;
Real[3] Lookup ditchSite;

Real Lookup time;


LibraryAction GOTO_POSITION (In String SearchType,
                             In Boolean TrafficConflict,
                             In Real TargetPosition[3],
                             In Real resSpeed,
                             In Integer PrevConflictId,
                             InOut Boolean resolutionCompleted);



DITCH:
{

   Boolean reqGuidance;
   Boolean success = false;
   Boolean restartDitching = false;
   Real resSpeed;
   Real captureH;
   EndCondition success;

   pprint("Plexil status:In Ditching plan");

   SetMode("ACTIVE");

   resSpeed  = Lookup(resolutionSpeed);
   captureH = resSpeed * 1.5;

   CORE:Concurrence{


       {
           Real waypoint[3];
           Repeat !success;
           success = Lookup(ditchingComplete);
           restartDitching = Lookup(resetDitching);


           // This has no reason to be here, but plexil will not let
           // the other actions under CORE run if only have two repeating lookups
           {
              EndCondition isKnown(waypoint[0]);
              waypoint = GetMissionWP(0);
           }

       }

       {
           Real ditchsite[3];
           Boolean validSite = false;
           Repeat !success;

           {
               Real waypoint[3];
               Repeat !validSite;

               ditchsite = Lookup(ditchSite);
               
               if(ditchsite[2] > -100.0){
                   pprint("Plexil status: received valid ditch site ",ditchsite[0],ditchsite[1],ditchsite[2]);
                   validSite = true;
               }endif

               // This has no reason to be here, but plexil will not let
               // the other actions under CORE run if only have two repeating lookups
               {
                    EndCondition isKnown(waypoint[0]);
                    waypoint = GetMissionWP(0);
               }

           }

           //pprint("Plexil status:checking guidance requirements");

           reqGuidance = Lookup(requireDitchGuidance);

           pprint("Plexil status:guidance requested:",reqGuidance);

           ICAROUS_GUIDED:{
              Boolean complete = false;
              Boolean rescomplete;
              StartCondition reqGuidance && !success;
              SkipCondition !reqGuidance;

              InvariantCondition (!restartDitching || !complete);

              pprint("Plexil status: Ditching with Icarous guidance");

              LibraryCall GOTO_POSITION(SearchType = "GRID",
                                        TrafficConflict = false,TargetPosition = ditchsite,
                                        resSpeed = resSpeed,
                                        PrevConflictId = 0,
                                        resolutionCompleted = rescomplete);
              complete = true;
           }

           DIRECT:{
              Boolean complete = false;
              StartCondition !reqGuidance && !success;
              SkipCondition reqGuidance;

              EndCondition (complete);

              pprint("Plexil status: Proceeding direct to ditch site");

              SetPos(ditchsite);

              complete = true;
           }

       }
   }

}